///////////////////////////////////////////////////////////////////////////////
// Name:        detectobjects.cpp
// Purpose:     Detect Objects
// Created:     2006
// Copyright:   (c) 2006, Ivan Laptev, TT Solutions
// License:     GPL, Please contact us for commercial license.
//              http://www.tt-solutions.com/
///////////////////////////////////////////////////////////////////////////////
#include "objectdet/detectobjects.h"

#include "objectdet/adacascadeclass.h"
#include "objectdet/dump.h"
#include "objectdet/integralhistogram.h"
#include "objectdet/profiling.h"
#include "objectdet/prune_subwindows.h"

#include <sstream>
#include <time.h>

using namespace std;

boost::tuple<pMat, pMat> ClassifyImagePixels(pMat source_image,
                                             pMat cluster_means_norm,
                                             PixClass pixclass)
{
    pMat timg = CreateMat(source_image->rows, source_image->cols, CV_16SC1);
    pMat wimg = CreateMat(source_image->rows, source_image->cols, CV_32FC1);
    ilClassifyImagePixels(source_image, timg, wimg, cluster_means_norm, (int)pixclass);
    return boost::make_tuple(timg, wimg);
}


arrMat IntegralBlockHist(pMat timg, pMat wimg,
                         size_t cluster_number,
                         size_t dx, size_t dy)
{
    int nby = (div(timg->rows, static_cast<int>(dy))).quot;
    int nbx = (div(timg->cols, static_cast<int>(dx))).quot;
    arrMat iblockhist(cluster_number);
    for (size_t i = 0; i < cluster_number; i++)
    {
        iblockhist[i] = CreateMat(nby, nbx, CV_32FC1);
    }

//    ilIntegralBlockHist2( timg, wimg, iblockhist, cluster_number, dx, dy);
//    Tick("ilIntegralBlockHist2");

    ilIntegralBlockHist( timg, wimg, iblockhist, cluster_number, dx, dy);
//    Tick("IntegralBlockHist");
    return iblockhist;
}

IMPEXP
SubWindows DetectObjects(pImage src,
                         Model const & model, 
                         pMat cmeans,
                         pMat covmat,
                         pMat cmeansnorm,
						 int pixclass,
						 HistBlockSizeArray bsarray,
						 ScaleFctArray scarray)
{
    using namespace std;

	//*** convert to one channel CvMat
    pMat srcgr = CreateMat(src->height, src->width, CV_8UC1);
	if (src->nChannels == 1)
    {
        CopyMatrix(ImageAsMatrix(src), srcgr);
    }
	else
    {
        CvtColor(ImageAsMatrix(src), srcgr, CV_RGB2GRAY);
    }

	SubWindows subwindows;

	//*** iterate over scales
	for (ScaleFctArray::iterator sc = scarray.begin(); sc != scarray.end(); ++sc){

		double scf =* sc;
		int ysz=(int)floor(scf * (src->height));
		int xsz=(int)floor(scf * (src->width));

		if ((xsz>0) && (ysz>0))
        {
			//*** subsample image
			//printf("xsz: %d, ysz: %d\n",xsz,ysz);
			pMat srcimg = CreateMat(ysz, xsz, CV_8UC1);
			Resize(srcgr, srcimg);
			pMat srcimg32f = CreateMat(ysz, xsz, CV_32FC1);
			Convert(srcimg, srcimg32f);	
	
			//********************************************************
			//***  MAIN PART FROM HERE....
			//***  COMPUTES HISTOGRAM OF IMAGE "srcimg"

			//*** label image pixels
			//int pixclass=TEXTONS_SC12_GDORDER2_;
			//int pixclass=ORIENTGRAD_DNUM4_;
			pMat timg = CreateMat(ysz,xsz,CV_16SC1);
			pMat wimg = CreateMat(ysz,xsz,CV_32FC1);

			ilClassifyImagePixels(srcimg, timg, wimg, cmeansnorm, pixclass);
			if (0) //DEBUG
            {
                SetValue(timg, cvScalar(1.0));
                SetValue(wimg, cvScalar(1.0));
			}
			
			//*** iterate over block sizes
			for (HistBlockSizeArray::iterator bs = bsarray.begin(); bs != bsarray.end(); ++bs)
            {	
				//*** compute integral weighted block histogram
				double dx=bs->dx, dy=bs->dy;
				int nc = 0;

				if ((dx<xsz) && (dy<ysz))
                {
					//printf("  - dx:%1.1f, dy:%1.1f\n",dx,dy);
					switch(pixclass)
                    {
                    case TEXTONS_SC12_GDORDER2_:
                        nc = 12;
                        break;
                    case ORIENTGRAD_DNUM4_:
                    default:
                        nc = 4;
                    }
					int	nbx = (int)floor((double)xsz/(double)dx);
					int nby = (int)floor((double)ysz/(double)dy);
					arrMat iblockhist(nc);
					for (int i=0; i<nc; i++)
                    {
                        iblockhist[i] = CreateMat(nby,nbx,CV_32FC1);
                    }
					// /*
					
					if(pixclass == ORIENTGRAD_DNUM4_)
                    {
						ilIntegralBlockHist(timg, wimg, iblockhist, nc, dx, dy); // weighted hist
					}
                    else
                    {
						ilIntegralBlockHist(timg, pMat(), iblockhist, nc, dx, dy); // unweighted hist
					}
//                    Tick("histogram computed");	

					//***  COMPLETED
					//***  HISTOGRAM IS IN "iblockhist"
					//********************************************************
		
					//*** detect objects
					unsigned int hist_size_x = iblockhist[0]->cols;
					unsigned int hist_size_y = iblockhist[0]->rows;
					unsigned int subwin_size_x = model.begin()->histblock_winsize_x;
					unsigned int subwin_size_y = model.begin()->histblock_winsize_y;
					unsigned int subwin_step_x = 1;
					unsigned int subwin_step_y = 1;    
					unsigned int top, left;
					
					if (hist_size_x>=subwin_size_x && hist_size_y>=subwin_size_y)
                    {
						unsigned int left_max = hist_size_x - subwin_size_x;     assert( left_max > 0 );
						unsigned int top_max  = hist_size_y - subwin_size_y;     assert( top_max > 0 );
						ClearModelCache(model);
						//*** classify each subwindow in the image
						for(left = 0; left < left_max; left += subwin_step_x)
                        {
							for(top = 0; top < top_max; top += subwin_step_y)
                            {
								SubWindowInfo result
                                    = adacascadeclass(model, iblockhist,
                                                      nc, left, top,
                                                      subwin_size_x, subwin_size_y);
								if (result.passed){ // stage>=stage_min){
									result.x = (unsigned int)((double)(result.x+1)*dx/scf);
									result.y = (unsigned int)((double)(result.y+1)*dy/scf);
									result.w = (int)(result.w * dx/scf);
									result.h = (int)(result.h * dy/scf);
									subwindows.push_back(result);
								}
							}
						} // iteration over all positions
					}
//                    Tick("Detection completed");
				}
			} // iteration over block sizes
		}
	} // iterarion over scales
	
	Tick("Total processing time");

	//*** pruning
	SubWindows prunedsubwindows;
    PruneSubWindows(subwindows, 0.3, 1, prunedsubwindows);

    return prunedsubwindows;
}

void ClearModelCache(Model const & model)
{
    for (Model::const_iterator it = model.begin(),
                              end = model.end();
                              it != end; ++it)
    {
        for (StageRules::const_iterator rit = it->rules.begin(),
                                       rend = it->rules.end();
                                       rit != rend; ++rit)
        {
            rit->clear_cache();
        }
    }
}

